#include "world/point.hpp"
#include "world/map.hpp"
#include "world/path.hpp"

#include <vector>
#include <queue> // priority_queue

#include <cmath>
#include <iostream>

#define cost_contiguous 10
#define cost_bidiagonal 14

using namespace std;
using namespace dw;

// return 0 for both 0 and inf
inline double slope(const Coord& p1, const Coord& p2) {
    if (1E-2 > fabs(p2.x - p1.x)) {
        return 0;
    } else {
        return (double)(p2.y - p1.y) / (p2.x - p1.x);
    }
}

// get Euclid distance
inline double distance(const Coord& p1, const Coord& p2) {
    return Vec(p1-p2).mag();
}

inline bool reach(const Coord& des, const Coord& p, double radius = 0.1) {
    return distance(des, p) <= radius;
}

/****************************
 *
 *  A* Algorithm: Heuristic Function
 *
 * **************************/
int PointAstar::moveCost(const Grid &p1, const Grid &p2)
{
    return (int)distance(p1, p2);
}

int PointAstar::estimateCost(const Grid &p1, const Grid &p2)
{
    return (int)distance(p1, p2);
}


/**************************
 *
 *          Path
 *
 * ************************/

// Debug: Output all visited Points
ostream& operator << (ostream& os, const vector<vector<PointAstar>>& m) {
    const int row = m.size();
    const int col = m[0].size();

    os << "Map Size (r * c): " << row << " x " << col << endl;

    os << setw(3) << "";
    for (int x = 0; x < col; x++)
    {
        os << setw(2) << x;
    }
    os << endl;

    for (int y = 0; y < row; y++)
    {
        os << setw(2) << y << " ";
        for (int x = 0; x < col; x++)
        {
            if (m[y][x].isVisited()) {
                os << Map::block[Map::Color::RED];
            } else {
                os << Map::block[Map::Color::WHITE];
            }
        }
        os << endl;
    }
    return os;
}

bool Path::operator()(
    Map::State &mapstate,
    const Coord &start, const Coord &end,
    const int scalar, const int radius)
{
    Map::State newMapstate;
    newMapstate = mapstate.scale(scalar);
    newMapstate = newMapstate.expand(radius);

    bool flag = (*this)(newMapstate, double(scalar) * start, double(scalar) * end);
    if (!flag) return false;

    auto iter = m_path.begin();
    *iter = start;
    for (++iter; iter != --m_path.end(); ++iter)
    {
        *iter /= double(scalar);
    }
    *iter = end;
    pCurrent = m_path.begin();
    
    return true;
}

// A* Algorithm
bool Path::operator()(const Map::State &mapstate, const Coord &start, const Coord &end)
{
    const int row = mapstate.row;
    const int col = mapstate.col;

    if (mapstate.map.empty())
	{
		cout << "Illegal map!" << endl;
		return false;
	}

    if (start == end)
    {
        cout << "The start point is exactly the end point!" << endl;
        return false;
    }

    if (!mapstate.contains(start) ||
        !mapstate.contains(end))
    {
		cout << "Illegal startpoint or endpoint" << endl;
		return false;
	}
		
    if (mapstate.isBlock(start) || mapstate.isBlock(end))
    {
		cout << "Illegal startpoint or endpoint" << endl;
		return false;
	}

    vector<vector<PointAstar>> pointArray(row, vector<PointAstar>(col));
	priority_queue<PointAstar*, vector<PointAstar*>, FromSmallToBig> openlist;

	PointAstar from(start);
    from.calCost(end);
    pointArray[start.y][start.x] = from;
    openlist.push(&pointArray[start.y][start.x]);

    const Coord directions[8] = {
        { 1,  0},
        { 1, -1},
        { 0, -1},
        {-1, -1},
		{-1,  0},
        {-1,  1},
        { 0,  1},
        { 1,  1}
    };

    bool seek = true;
    while (seek && !openlist.empty())
    {
		PointAstar* pCurrent = openlist.top();
        pCurrent->calCost(end);
        openlist.pop();

        for (int i = 0; i < 8; ++i)
		{
            Coord next = pCurrent->coord() + directions[i];
            
            // Out of map || Visited || Obstacle
            if (!mapstate.contains(next) ||
                pointArray[next.y][next.x].isVisited() ||
                mapstate.isBlock(next))
            {
                continue;
            }

			PointAstar son(next, pCurrent);
            son.calCost(end);
            pointArray[next.y][next.x] = son;
            openlist.push(&pointArray[next.y][next.x]);

            // Arrived at end
			if (end == son.coord()) {
                seek = false;
                break;
            }
        }
    }

	if (!pointArray[end.y][end.x].isVisited())
	{
		cout << "Unreachable!" << endl;
        return false;
    }
    else
	{
        this->m_path.clear();
        // Get Path by Recalling
        PointAstar* current = &pointArray[end.y][end.x];
        do {
            this->push_front(current->coord());
            current = current->parent();
        } while (nullptr != current);

        this->pCurrent = m_path.begin();
    }

    return true;
}


bool Path::show(const Map::State& map)
{
    cout << *this << endl;
    return true;
}

bool Path::checkCrossWalkable(const Map::State &mapstate, const Coord &p1, const Coord &p2)
{
    const int row = mapstate.row;
    const int col = mapstate.col;

    double k = slope(p1, p2);
    double b = p1.y - k * p1.x;

    if (k != 0)
    {
        if (k > 1 || k < -1)
        {
            b = -b / k;
            k = 1 / k;

            Coord lower = (p1.y < p2.y) ? p1 : p2;
            Coord upper = p1 + p2 - lower;

            for (int y = (int)floor(lower.y); y < (int)ceil(upper.y); ++y)
            {
                double x = k * y + b;
                int int_x = (int)floor(x);
                
                if (mapstate.isBlock({int_x, y})) return false;
                if (mapstate.isBlock({int_x, y - 1})) return false;

                if (x < int_x + 0.25)
                {
                    if (mapstate.isBlock({int_x - 1, y})) return false;
                    if (mapstate.isBlock({int_x - 1, y - 1})) return false;
                }
                else if (x > int_x + 0.75)
                {
                    if (mapstate.isBlock({int_x + 1, y})) return false;
                    if (mapstate.isBlock({int_x + 1, y - 1})) return false;
                }
            }
        }
        else
        {
            Coord left = (p1.x < p2.x) ? p1 : p2;
            Coord right = p1 + p2 - left;

            for (int x = (int)floor(left.x); x < (int)ceil(right.x); ++x)
            {
                double y = k * x + b;
                int int_y = (int)floor(y);

                if (mapstate.isBlock({x, int_y})) return false;
                if (mapstate.isBlock({x - 1, int_y})) return false;

                if (y < int_y + 0.25)
                {
                    if (mapstate.isBlock({x, int_y - 1})) return false;
                    if (mapstate.isBlock({x - 1, int_y - 1})) return false;
                }
                else if (y > int_y + 0.75)
                {
                    if (mapstate.isBlock({x, int_y + 1})) return false;
                    if (mapstate.isBlock({x - 1, int_y + 1})) return false;
                }
            }
        }
    }
    else
    {
        if (1E-2 > fabs(p1.x - p2.x))
        {
            Coord lower = (p1.y < p2.y) ? p1 : p2;
            Coord upper = p1 + p2 - lower;

            for (int y = (int)floor(lower.y); y < (int)ceil(upper.y); ++y)
            {
                int int_x = (int)floor(p1.x);
                if (mapstate.isBlock({int_x, y})) return false;

                if (p1.x > int_x + 0.75) {
                    if (mapstate.isBlock({int_x + 1, y})) return false;
                }
                if (p1.x < int_x + 0.25) {
                    if (mapstate.isBlock({int_x - 1, y})) return false;
                }
            }
        }
        else if (1E-2 > fabs(p1.y - p2.y))
        {
            Coord left = (p1.x < p2.x) ? p1 : p2;
            Coord right = p1 + p2 - left;

            for (int x = (int)floor(left.x); x < (int)ceil(right.x); ++x)
            {
                int int_y = (int)floor(p1.y);
                if (mapstate.isBlock({x, int_y})) return false;

                if (p1.y > int_y + 0.75) {
                    if (mapstate.isBlock({x, int_y + 1})) return false;
                }
                if (p1.y < int_y + 0.25) {
                    if (mapstate.isBlock({x, int_y - 1})) return false;
                }
            }
        }
    }

    return true;
}

Path &Path::simplify(const Map::State &mapstate)
{
    if (empty()) return *this;
    
    using Vec = Coord;
    
    // Remove Colinear Points
    if (m_path.size() > 2)
    {
        auto last_1 = --m_path.end();
        auto last_2 = last_1; --last_2;

        Vec v = *last_1 - *last_2;
        Vec temp;

        bool remove = false;
        const Coord discarded = Coord(-1, -1);
        
        for (auto i = --last_2; i != m_path.begin(); --i) {
            auto j = i;
            --j;

            temp = *i - *j;
            if (v.x * temp.y - v.y * temp.x == 0) {
                *i = discarded;
                remove = true;
            }
            else
            {
                v = temp;
            }
        }

        if (remove) {
            m_path.remove(discarded);
        }
    }

    // Remove redundant turning points
    bool remove = false;
    const Coord discarded = Coord(-1, -1);

    for (auto i = --m_path.end(); i != m_path.begin(); --i)
    {
        if (discarded == *i) continue;

        for (auto j = m_path.begin(); j != i; ++j)
        {
            if (discarded == *j) continue;

            if (checkCrossWalkable(mapstate, *i, *j)) 
            {
                remove = true;
                for (auto k = ++j; k != i; ++k)
                {
                    *k = discarded;
                }

                i = j;
                break;
            }
        }
    }

    if (remove)
    {
        m_path.remove(discarded);
    }

    return *this;
}

Path& Path::interpolate(uint32_t num) {
    // todo
    return *this;
}

Path& Path::interpolate(double dis) {
    Coord p1 = this->start();

    Path path;
    path.push_back(p1);

    for (auto iter = ++(this->begin()); iter != this->end(); ++iter) {
        Coord p2 = *iter;
        double distance = Vec(p2 - p1).mag();

        int cnt = 0;
        double interval = dis;
        if (distance > dis) {
            cnt = int(distance / interval);
            interval = distance / cnt;
        }

        for (int i = 1; i < cnt; ++i) {
            Coord mid = (double(i) * p2 + double(cnt - i) * p1) / double(cnt);
            path.push_back(mid);
        }
        path.push_back(p2);
        p1 = p2;
    }

    this->m_path.clear();
    this->m_path = path.m_path;
    this->pCurrent = this->begin();

    return *this;
}

string Path::encode()
{
    nlohmann::json j;
    to_json(j["path"], *this);
    return j.dump();
}

Path Path::decode(string data_msg)
{
    nlohmann::json j = nlohmann::json::parse(data_msg);
    from_json(j["path"], *this);
    return *this;
}

namespace dw
{
    void to_json(nlohmann::json &j, const Path &path)
    {
        j["nodes"] = nlohmann::json::array();
        for (auto coord : path.m_path)
        {
            j["nodes"] += nlohmann::json{
                {"x", coord.x},
                {"y", coord.y}
            };
        }
        j["length"] = path.length();
    }

    void from_json(const nlohmann::json &j, Path &path)
    {
        if (j["length"].is_null() || 0 == j["length"]) {
            path.m_path.clear();
            path.pCurrent = path.m_path.end();
        } else {
            nlohmann::json nodes = j["nodes"];
            path.m_path.clear();
            for (auto jcoord : nodes)
            {
                path.m_path.emplace_back(Coord({jcoord["x"], jcoord["y"]}));
            }
        }
    }

    ostream &operator<<(ostream &os, const Path &path)
    {
        int cnt = 0;

        auto head = path.m_path.begin();
        auto tail = path.m_path.end();

        os << "Lately Passed Point: " << *path.pCurrent << endl;
        os << "From " << *head << " to " << *tail << endl;

        for (auto iter = head; iter != tail; ++iter)
        {
            if (!cnt)
                os << "\t";

            os << *iter << " -> ";

            ++cnt;
            if (cnt >= 10)
            {
                cnt = 0;
                os << endl;
            }
        }
        return os;
    }

    void showPathOnMap(Map &map, const Path &path)
    {
        for (auto &p : path)
        {
            map[p] = 2;
        }

        cout << map << endl;

        for (auto &p : path)
        {
            map[p] = 0;
        }
    }
}